﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using DotNetNinjaLibrary.Basics;

namespace DotNetNinjaLibrary.Pathfinding
{
	public class PathFinding
	{
		public static IEnumerable<Basics.Point> GetPath(MapModel map, Size unitSize, Basics.Point start, Basics.Point destination)
		{
			var open = new List<Basics.Point> { start };
			var treeOfParents = new Basics.Point[map.Height, map.Width];

			var g = new float[map.Height, map.Width];

			for (int i = 0; i < map.Height; i++)
				for (int j = 0; j < map.Width; j++)
				{
					g[i, j] = 0;
					treeOfParents[i, j] = new Basics.Point(-1, -1);
				}

			treeOfParents[start.Y, start.X] = start;

			while (open.Any())
			{
				int indexOfMin = FindIndexOfMin(g, open, destination);
				var point = open[indexOfMin];
				open.RemoveAt(indexOfMin);

				if (point == destination)
				{
					var result = new List<Basics.Point>();

					while (point != start)
					{
						result.Insert(0, point);
						point = treeOfParents[point.Y, point.X];
					}

					return result;
				}

				foreach (var next in GenerateNextMoves(point).Where(p => map.IsMovePossible(p, unitSize)))
				{
					if (treeOfParents[next.Y, next.X].X == -1)
					{
						g[next.Y, next.X] = g[point.Y, point.X] + Distance(point, next);
						treeOfParents[next.Y, next.X] = point;

						open.Add(next);
					}
				}
			}

			return Enumerable.Empty<Basics.Point>();
		}

		private static float Distance(Basics.Point point, Basics.Point next)
		{
			return (float)Math.Sqrt((next.X - point.X) * (next.X - point.X) + (next.Y - point.Y) * (next.Y - point.Y));
		}

		private static int CalculateH(Basics.Point point, Basics.Point destination)
		{
			return Math.Max(Math.Abs(destination.X - point.X), Math.Abs(destination.Y - point.Y));
		}

		private static int FindIndexOfMin(float[,] g, List<Basics.Point> open, Basics.Point destination)
		{
			var minIndex = 0;
			var currentMin = CalculateH(open[0], destination) + g[open[0].Y, open[0].X];
			for (int i = 1; i < open.Count; i++)
			{
				var currentValue = CalculateH(open[i], destination) + g[open[i].Y, open[i].X];
				if (currentMin > currentValue)
				{
					minIndex = i;
					currentMin = currentValue;
				}
			}
			return minIndex;
		}

		public static IEnumerable<Basics.Point> GenerateNextMoves(Basics.Point start)
		{
			yield return new Basics.Point(start.X - 1, start.Y - 1);
			yield return new Basics.Point(start.X, start.Y - 1);
			yield return new Basics.Point(start.X + 1, start.Y - 1);
			yield return new Basics.Point(start.X + 1, start.Y);
			yield return new Basics.Point(start.X + 1, start.Y + 1);
			yield return new Basics.Point(start.X, start.Y + 1);
			yield return new Basics.Point(start.X - 1, start.Y + 1);
			yield return new Basics.Point(start.X - 1, start.Y);
		}
	}
}